#pragma once

#include "hnurm_detect/detector.hpp"

#include <hnurm_interfaces/msg/armor_array.hpp>
#include <hnurm_interfaces/msg/vision_recv_data.hpp>

#include <image_transport/image_transport.hpp>
#include <sensor_msgs/msg/image.hpp>

#include <std_msgs/msg/float64.hpp>

#include <message_filters/subscriber.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>

namespace hnurm
{

hnurm_interfaces::msg::Armor toROSMsg(const Armor &armor);

class DetectNode : public rclcpp::Node
{
public:
    explicit DetectNode(const rclcpp::NodeOptions &options);

    void init_params();

private:
    void detect_callback(
        sensor_msgs::msg::Image::SharedPtr img_msg, hnurm_interfaces::msg::VisionRecvData::SharedPtr uart_msg
    );

private:
    using ApproximateTimePolicy = message_filters::sync_policies::
        ApproximateTime<sensor_msgs::msg::Image, hnurm_interfaces::msg::VisionRecvData>;
    // subs
    std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::Image>>               sub_img_;
    std::shared_ptr<message_filters::Subscriber<hnurm_interfaces::msg::VisionRecvData>> sub_uart_;
    std::shared_ptr<message_filters::Synchronizer<ApproximateTimePolicy>>               sync_;

    image_transport::Publisher                                      pub_res_img_;
    image_transport::Publisher                                      pub_num_img_;
    image_transport::Publisher                                      pub_bin_img_;
    rclcpp::Publisher<hnurm_interfaces::msg::ArmorArray>::SharedPtr pub_res_armor_;

    // debug pubs
    bool pub_debug_info_ = true;
    rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr pub_detect_time_;

    // topics
    std::string recv_topic_;
    std::string image_topic_;
    std::string res_img_topic_;
    std::string num_img_topic_;
    std::string bin_img_topic_;
    std::string res_armor_topic_;

    // detector
    std::shared_ptr<Detector>  detector_;
    int                        min_lightness_ = 120;
    Detector::LightParams      light_params_ {};
    Detector::ArmorParams      armor_params_ {};
    Detector::ClassifierParams classifier_params_ {};
};
}
